package sample;

import rescuecore2.worldmodel.EntityID;
import rescuecore2.worldmodel.Entity;
import rescuecore2.misc.collections.LazyMap;

import java.util.List;
import java.util.LinkedList;
import java.util.Arrays;
import java.util.Set;
import java.util.HashSet;
import java.util.Map;
import java.util.HashMap;
import java.util.Collection;
import java.util.Comparator;
import java.util.PriorityQueue;
import java.util.Queue;

import rescuecore2.standard.entities.StandardWorldModel;
import rescuecore2.standard.entities.Area;

/**
   A sample search class that uses a connection graph to look up neighbours.
 */
public final class SampleSearch {
    private Map<EntityID, Set<EntityID>> graph;
    private StandardWorldModel world;

    /**
       Construct a new SampleSearch.
       @param world The world model to construct the neighbourhood graph from.
    */
    public SampleSearch(StandardWorldModel world) {
        this.world = world;
        Map<EntityID, Set<EntityID>> neighbours = new LazyMap<EntityID, Set<EntityID>>() {
            @Override
            public Set<EntityID> createValue() {
                return new HashSet<EntityID>();
            }
        };
        for (Entity next : this.world) {
            if (next instanceof Area) {
                Collection<EntityID> areaNeighbours = ((Area)next).getNeighbours();
                neighbours.get(next.getID()).addAll(areaNeighbours);
            }
        }
        setGraph(neighbours);
    }


    /**
       Construct a new ConnectionGraphSearch.
       @param graph The connection graph in the form of a map from EntityID to the set of neighbouring EntityIDs.
     */
    public SampleSearch(Map<EntityID, Set<EntityID>> graph) {
        setGraph(graph);
    }

    /**
       Set the neighbourhood graph.
       @param newGraph The new neighbourhood graph.
    */
    public void setGraph(Map<EntityID, Set<EntityID>> newGraph) {
        this.graph = newGraph;
    }

    /**
       Get the neighbourhood graph.
       @return The neighbourhood graph.
    */
    public Map<EntityID, Set<EntityID>> getGraph() {
        return graph;
    }

    /**
        Algoritmo de busca A*
       @param start A localização inicial
       @param goals O conjunto de objetivos
       @return O caminho entre a origem e um dos destinos (null se não for houver caminho possível)
    */
    public List<EntityID> aStarSearch(EntityID start, EntityID... goals) {
        return aStarSearch(start, Arrays.asList(goals));
    }

    /**
       Algoritmo de busca A*
       @param start A localização inicial
       @param goals O conjunto de objetivos
       @return O caminho entre a origem e um dos destinos (null se não for houver caminho possível)
    */
    public List<EntityID> aStarSearch(EntityID start, Collection<EntityID> goals) {
        // Se não houver uma lista de objetivos, retorna null
        if(goals.isEmpty())
            return null;
        Comparator<Node> comparator = new HeuristicComparator();
        Queue<Node> open = new PriorityQueue<Node>(10,comparator);
        List<Node> closed = new LinkedList<Node>();
        // HashMap utilizado para guardar o grafo percorrido
        Map<EntityID, EntityID> ancestors = new HashMap<EntityID, EntityID>();
        // Nó origem
        Node startNode = new Node(start, 0, getDistanceFromGoal(start, goals));
        open.add(startNode);
        Node next = null;
        boolean found = false;
        ancestors.put(start, start);
        do {
            next = open.remove();
            closed.add(next);
            // Se o objetivo for encontrado, sai do laço
            if (isGoal(next.id, goals)) {
                found = true;
                break;
            }
            // Verifica a lista de vizinhos do nó em questão
            Collection<EntityID> neighbours = graph.get(next.id);
            if (neighbours.isEmpty()) {
                continue;
            }

            for (EntityID neighbour : neighbours) {
                if (isGoal(neighbour, goals)) {
                    ancestors.put(neighbour, next.id);
                    next = new Node(neighbour, 0, 0);
                    found = true;
                    break;
                }
                // Cria um nó para o vizinho em questão
                // Caso o destino e a origem sejam os mesmos, utiliza o valor 0 em vez 
                // de -1 (da função getDistance)
                long distance = world.getDistance(next.id, neighbour);
                if(distance < 0)
                    distance = 0;
                Node y = new Node(neighbour, next.g_n + distance, getDistanceFromGoal(neighbour, goals));
                
                if(closed.contains(y)){
                    continue;
                }

                else if (!ancestors.containsKey(y.id) && !open.contains(y)){
                    open.add(y);
                    ancestors.put(y.id, next.id);
                }
            }
        } while (!found && !open.isEmpty());
        if (!found) {
            // Caminho inexistente
            return null;
        }
        // Reconstrói o caminho do destino para a origem
        EntityID current = next.id;
        List<EntityID> path = new LinkedList<EntityID>();
        do {
            path.add(0, current);
            current = ancestors.get(current);
            if (current == null) {
                throw new RuntimeException("Found a node with no ancestor! Something is broken.");
            }
        } while (current != start);
        return path;
    }

    /**
     * Distancia entre um nó e o objetivo mais próximo
     * @param node Nó a ser testado
     * @param goals Lista de objetivos
     * @return Distância
     */
    private long getDistanceFromGoal(EntityID node, Collection<EntityID> goals){
        long dMin = Long.MAX_VALUE;
        for(EntityID g : goals){
            long currentDist = world.getDistance(node, g);
            // Se currentDist for menor que zero, significa que o destino e a origem são iguais
            if(currentDist < 0)
                return 0;
            if(currentDist < dMin){
                dMin = currentDist;
            }
        }
        return dMin;
    }

    /**
       Do a breadth first search from one location to the closest (in terms of number of nodes) of a set of goals.
       @param start The location we start at.
       @param goals The set of possible goals.
       @return The path from start to one of the goals, or null if no path can be found.
    */
    public List<EntityID> breadthFirstSearch(EntityID start, EntityID... goals) {
        return breadthFirstSearch(start, Arrays.asList(goals));
    }

    /**
       Do a breadth first search from one location to the closest (in terms of number of nodes) of a set of goals.
       @param start The location we start at.
       @param goals The set of possible goals.
       @return The path from start to one of the goals, or null if no path can be found.
    */
    public List<EntityID> breadthFirstSearch(EntityID start, Collection<EntityID> goals) {
        List<EntityID> open = new LinkedList<EntityID>();
        Map<EntityID, EntityID> ancestors = new HashMap<EntityID, EntityID>();
        open.add(start);
        EntityID next = null;
        boolean found = false;
        ancestors.put(start, start);
        do {
            next = open.remove(0);
            if (isGoal(next, goals)) {
                found = true;
                break;
            }
            Collection<EntityID> neighbours = graph.get(next);
            if (neighbours.isEmpty()) {
                continue;
            }
            for (EntityID neighbour : neighbours) {
                if (isGoal(neighbour, goals)) {
                    ancestors.put(neighbour, next);
                    next = neighbour;
                    found = true;
                    break;
                }
                else {
                    if (!ancestors.containsKey(neighbour)) {
                        open.add(neighbour);
                        ancestors.put(neighbour, next);
                    }
                }
            }
        } while (!found && !open.isEmpty());
        if (!found) {
            // No path
            return null;
        }
        // Walk back from goal to start
        EntityID current = next;
        List<EntityID> path = new LinkedList<EntityID>();
        do {
            path.add(0, current);
            current = ancestors.get(current);
            if (current == null) {
                throw new RuntimeException("Found a node with no ancestor! Something is broken.");
            }
        } while (current != start);
        return path;
    }

    private boolean isGoal(EntityID e, Collection<EntityID> test) {
        return test.contains(e);
    }

    // Comparator utilizado para ordenar a fila de nós pela heurística
    class HeuristicComparator implements Comparator<Node>{
        public int compare(Node t1, Node t2) {
            if (t1.getFn() < t2.getFn()){
                return -1;
            }
            if (t1.getFn() > t2.getFn()){
                return 1;
            }
            return 0;
        }
    }

    // Classe node, com variáveis para Id, g(n) e h(n)
    class Node{
        EntityID id;
        long g_n;
        long h_n;

        public Node(EntityID id, long g_n, long h_n) {
            this.id = id;
            this.g_n = g_n;
            this.h_n = h_n;
        }

        public long getFn(){
            return g_n + h_n;
        }
    }
}
